import sys
print("当前 sys.path 前几项：")
for p in sys.path[:6]:
    print(p)

import numpy as np
print("NumPy 实际路径：", np.__file__)
print("NumPy 版本：", np.__version__)

import threading
print("threading 文件路径:", threading.__file__)
print("是否有 _set_sentinel:", hasattr(threading, '_set_sentinel'))

import time
# import threading
import serial
# import sys
import os

# ==================== 【关键修复】把路径和 import 提到最前面 ====================
# 立即添加当前目录到 Python 模块搜索路径
current_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, current_dir)

# 现在再导入 BrainLinkParser（必须在 sys.path 修改之后）
try:
    from BrainLinkParser import BrainLinkParser
    print("✅ BrainLinkParser 模块加载成功！")
except ImportError as e:
    print("❌ 无法导入 BrainLinkParser！")
    print("   可能原因：")
    print("   1. Python 版本不是 3.11（官方要求必须 3.11）")
    print("   2. BrainLinkParser.pyd 文件不在当前文件夹")
    print("   3. 文件名不是 BrainLinkParser.pyd（注意大小写）")
    print(f"   当前 Python 版本: {sys.version}")
    print(f"   当前搜索路径: {sys.path[0]}")
    sys.exit(1)
# ===========================================================================

from djitellopy import Tello

# ==================== 配置区 ====================
BRAIN_PORT = "COM6"          # ←←←←← 这里改成你 Windows 的实际输出 COM 端口
                             # （设备管理器 → 端口 → BrainLink 的“输出”端口，通常是 COM3/COM5/COM7）

# 控制阈值（0-100），可自行调整
ATTENTION_THRESHOLD = 80   # 注意力超过此值 → 起飞
MEDITATION_THRESHOLD = 80  # 冥想超过此值 → 降落

DEBOUNCE_SEC = 4           # 防误触冷却时间（秒）
# ===============================================

# 全局变量
latest_data = None
data_lock = threading.Lock()
is_flying = False
last_action_time = 0

# BrainLink 回调函数
def on_eeg(data):
    global latest_data
    with data_lock:
        latest_data = data
    print(f"【实时脑波】注意力: {data.attention:3d} | 冥想: {data.meditation:3d} | "
          f"delta: {data.delta:4d} theta: {data.theta:4d}")

def on_extend_eeg(data):
    print(f"【扩展信息】电量: {data.battery}%  温度: {data.temperature}°C")

def on_gyro(x, y, z): pass
def on_rr(rr1, rr2, rr3): pass
def on_raw(raw): pass

# 串口读取线程
def serial_reading_thread(port):
    global parser
    try:
        ser = serial.Serial(port, 115200, timeout=1)
        print(f"✅ BrainLink 串口已打开: {port}")
        
        parser = BrainLinkParser(on_eeg, on_extend_eeg, on_gyro, on_rr, on_raw)
        
        while True:
            if ser.in_waiting > 0:
                byte_data = ser.read(ser.in_waiting)
                if byte_data:
                    parser.parse(byte_data)
            time.sleep(0.01)
    except Exception as e:
        print(f"❌ 串口错误: {e}")
        sys.exit(1)

if __name__ == "__main__":
    # 启动 BrainLink 读取线程
    threading.Thread(target=serial_reading_thread, args=(BRAIN_PORT,), daemon=True).start()
    
    # 初始化 Tello
    tello = Tello()
    tello.connect()
    print(f"✅ Tello 已连接，当前电量: {tello.get_battery()}%")
    
    print("\n🚀 简单版脑控无人机已就绪！")
    print(f"   注意力 > {ATTENTION_THRESHOLD} → 自动起飞")
    print(f"   冥想 > {MEDITATION_THRESHOLD} → 自动降落")
    print("   Ctrl+C 安全退出\n")
    
    try:
        while True:
            current_time = time.time()
            
            with data_lock:
                if latest_data is None:
                    time.sleep(0.3)
                    continue
                
                att = latest_data.attention
                med = latest_data.meditation
                now = current_time
                
                # 防抖 + 执行命令
                if now - last_action_time > DEBOUNCE_SEC:
                    if att >= ATTENTION_THRESHOLD and not is_flying:
                        print("🧠 高注意力检测 → 执行【起飞】")
                        try:
                            tello.takeoff()
                            is_flying = True
                            last_action_time = now
                        except Exception as e:
                            print(f"起飞失败: {e}")
                    
                    elif med >= MEDITATION_THRESHOLD and is_flying:
                        print("🧘 高冥想检测 → 执行【降落】")
                        try:
                            tello.land()
                            is_flying = False
                            last_action_time = now
                        except Exception as e:
                            print(f"降落失败: {e}")
            
            time.sleep(0.2)
    
    except KeyboardInterrupt:
        print("\n⛔ 用户中断，安全降落...")
        if is_flying:
            try:
                tello.land()
            except:
                pass
        print("程序已安全退出！")